#include <iostream>
#include <set>
#include <ctime>
#include <map>
#include <limits>
#include <vector>
#include <list>
#include <stack>
#include <math.h>

using namespace std;

const int N = 100;
const int M = 100;
const double S1 = 100;
const double S2 = 10;

const double dPi = 6.2831853;

struct Vertex {
  public:
    double x;
    double y;
    bool closed;
    double cost;
    Vertex* parent;

    Vertex(): x(0), y(0), closed(false), cost(0), parent(NULL) {}

    double getDistanse(Vertex* v) {
        return sqrt(pow(x - v->x, 2) + pow(y - v->y, 2));
    }
};

void generateGraph(vector<Vertex>& graph) {
    vector<Vertex> start_points(N);

    for (int i = 0; i < N; ++i) {
        double d1 = 1.0 - (double)(rand()) / (RAND_MAX + 1);
        double COS = cos(dPi * d1);
        double LOG = log(d1);

        start_points[i].x = S1 * COS * sqrt(-2 * LOG);
        start_points[i].y = S2 * sqrt(1.0 - COS * COS) * sqrt(-2 * LOG);
    }

    for (int i = 0; i < N; ++i) {
        for (int j = 0; j < M; ++j) {
            double d2 = 1.0 - (double)(rand()) / (RAND_MAX + 1);
            double COS = cos(dPi * d2);
            double LOG = log(d2);

            graph[i * M + j].x = S2 * COS * sqrt(-2 * LOG) + start_points[i].x;
            graph[i * M + j].y = S2 * sqrt(1 - COS * COS) * sqrt(-2 * LOG) + start_points[i].y;
        }
    }
}

double closeNeighbour(vector<Vertex>& graph) {
    vector<Vertex*> way;
    for (vector<Vertex>::iterator i = graph.begin(); i != graph.end(); ++i) {
        i->closed = false;
        i->parent = NULL;
    }

    Vertex* start = &graph[0];
    Vertex* current = start;
    vector<Vertex>::iterator preEnd = --graph.end();

    double summ = 0;
    Vertex* old = start;
    for (vector<Vertex>::iterator i = graph.begin(); i != preEnd; ++i) {
        current->closed = true;
        way.push_back(current);
        summ += old->getDistanse(current);
        old = current;

        double minimum = numeric_limits<double>::max();
        Vertex* minVertex = NULL;

        for (vector<Vertex>::iterator j = graph.begin(); j != graph.end(); ++j) {
            if (!j->closed) {
                double distance = current->getDistanse(&(*j));
                if (distance < minimum) {
                    minimum = distance;
                    minVertex = (&(*j));
                }
            }
        }
        minVertex->parent = current;
        current = minVertex;
    }
    start->parent = current;
    way.push_back(current);
    summ += old->getDistanse(current);
    summ += way[way.size()-1]->getDistanse(way[0]);

    return summ;
}

void getMinSpanningTree(vector<Vertex>& graph, map< Vertex* , list<Vertex*> >& graphRes) {
    //Алгоритм Прима
    for (vector<Vertex>::iterator i = graph.begin(); i != graph.end(); ++i){
        i->parent = NULL;
        i->cost = std::numeric_limits<double>::infinity();
        i->closed = false;
    }

    Vertex* start = &(*graph.begin());
    start->cost = 0;

    std::multimap<double, Vertex*> queue;
    for (vector<Vertex>::iterator i = graph.begin(); i != graph.end(); ++i){
        queue.insert(make_pair(i->cost, &(*i)));
    }

    Vertex *v, *u;

    while (!queue.empty()){
        v = queue.begin()->second;
        queue.erase(queue.begin());
        v->closed = true;

        for (vector<Vertex>::iterator i = graph.begin(); i != graph.end(); ++i) {
            u = &(*i);
            if (u->closed) { continue; }

            std::multimap<double, Vertex*>::iterator low = queue.lower_bound(u->cost);
            std::multimap<double, Vertex*>::iterator up = queue.upper_bound(u->cost);

            for (; low != up; ++low ) {
                if (low->second == u) { break; }
            }

            double distanse =  v->getDistanse(u);
            if (distanse < u->cost) {
                queue.erase(low);
                u->cost = distanse;
                u->parent = v;
                queue.insert(make_pair(distanse, u));
            }
        }

        if (v->parent != NULL && v != NULL) {
            graphRes[v->parent].push_back(v);
            graphRes[v].push_back(v->parent);
        }
    }
}

void eulerianPath(map< Vertex* , list<Vertex*> >& graphRes, vector< Vertex*>& way) {
     //Ищем Эйлеров цикл
     stack<Vertex*> stackS;
     Vertex* strt = graphRes.begin()->first;
     set< pair<Vertex*, Vertex*>  > closed;

     stackS.push(strt);
     while (!stackS.empty()) {
           Vertex* u = stackS.top();
           bool allClosed = true;
           for (list<Vertex*>::iterator i = graphRes[u].begin(); i != graphRes[u].end(); ++i) {
               pair<Vertex*, Vertex*> pr = make_pair(u,*i);
               if (closed.find(pr) == closed.end()) {
                   allClosed = false;
                   closed.insert(pr);
                   stackS.push(*i);
               }
           }
           if (allClosed) {
               way.push_back(u);
               stackS.pop();
           }
     }

}

double travellingSalesman(vector<Vertex>& graph, vector<Vertex*>& newWay) {
    // "Деревянный алгоритм": строим MST, по нему Эйлеров цикл,
    // после этого идем по циклу и печатаем каждую вершину 1 раз. Получившийся путь - искомый.

    double summ = 0;

    map< Vertex*, list<Vertex*> > MST;
    getMinSpanningTree(graph, MST);

    vector< Vertex*> way;
    eulerianPath(MST, way);

    for (vector<Vertex*>::iterator i = way.begin();  i != way.end(); ++i) {
        (*i)->closed = false;
    }

    Vertex* old;
    Vertex* next = *(way.begin());
    newWay.push_back(next);
    next->closed = true;
    for (vector<Vertex*>::iterator i = ++way.begin();  i != way.end(); ++i) {
        old = next;
        while (i != way.end() && (*i)->closed) { i++; }
        if (i == way.end()) { break; }
        next = *i;
        next->closed = true;
        newWay.push_back(next);
    }

    old = *newWay.begin();
    for( std::vector<Vertex*>::iterator y = ++newWay.begin(); y != newWay.end(); ++y) {
         summ += old->getDistanse(*y);
         old = *y;
    }
    summ += newWay[newWay.size()-1]->getDistanse(newWay[0]);
    return summ;
}

int main() {
    srand(time(NULL));
    vector<Vertex> graph(N * M);
    generateGraph(graph);

    clock_t t = clock();
    vector< Vertex*> way;
    cout<<"Prima weight: "<<travellingSalesman(graph, way)<<endl;
    cout<<"Prima time: "<<(double)((clock() - t)) / CLOCKS_PER_SEC<<endl<<endl;

    t = clock();
    cout<<"Close Neighbour weight: "<<closeNeighbour(graph)<<endl;
    cout<<"Close Neighbour time: "<<(double)((clock() - t)) / CLOCKS_PER_SEC<<endl<<endl;

    system("pause");
    return 0;
}
